% clc;
% clear
% close all;
% load('testdata_pll_5.mat');
% PLOT_LEN = 1200;
% ZERO_LEN = 0;%0 is the best fine tune
% 
% signal_rx = floor(signal(1:5000));
% figure(1);subplot(311);plot((0:PLOT_LEN-1)/60e6,imag(signal_rx(1:PLOT_LEN)));
% figure(2);subplot(311);plot((0:PLOT_LEN-1)/60e6,real(signal_rx(1:PLOT_LEN)));
% 
% signal_rx = downsample([zeros(ZERO_LEN,1);signal_rx], 6);
% signal_rx(1:51) = 0;
% figure(1);subplot(312);plot((0:PLOT_LEN/6-1)/10e6,imag(signal_rx(1:PLOT_LEN/6)),'-*');
% figure(2);subplot(312);plot((0:PLOT_LEN/6-1)/10e6,real(signal_rx(1:PLOT_LEN/6)),'-*');
% 
% rcos_fir = round(1.15 * rcosdesign(0.5, 6, 4, 'sqrt') * 2^15);
% signal_rx = floor(filter(rcos_fir, 1, signal_rx) / (2^16));
% signal_rx = [0;signal_rx];
% figure(1);subplot(313);plot((0:PLOT_LEN/6-1)/10e6,imag(signal_rx(1:PLOT_LEN/6)),'-*');
% figure(2);subplot(313);plot((0:PLOT_LEN/6-1)/10e6,real(signal_rx(1:PLOT_LEN/6)),'-*');

data = rx_shape;
% data = awgn(2^11*exp(1i*2*pi*1e3*(0:1e5-1)/10e6)*0.6,15,'measured');
A = log(1)*2^11; % desired pwr = 1w, A = ln(desired_pwr) = 0
g = 0; % gain init
N = 128; % update period
K= 1/2^12; % step size
yout = zeros(1,length(data));% output data after agc
window = zeros(1,N); % sliding window calculating the pwr of N signals
p_mean_pwr = zeros(1,length(data));
p_z = zeros(1,length(data));
p_sum = zeros(1,length(data));
p_g = zeros(1,length(data));
p_temp = zeros(1,length(data));
out_window = zeros(1,N);
p_cnt = 0;
locked_index = 0;
for i = 1:length(data)
    window = [window(2:end),(real(data(i))^2+imag(data(i))^2)/2^11]; % load latest data into window
    
    if(i>length(window))  
        z = log(sum(window)/N/2^11)*2^11; % calculate the mean(window) and the log(mean)

        yout(i) = data(i)*exp(g/2^11); % output = input * gain
        out_window = [out_window(2:end),(real(yout(i))^2+imag(yout(i))^2)/2^11];
        p_temp(i) = mean(out_window);
        if(p_temp(i) <= 2200 && p_temp(i)>=1800)
            p_cnt = p_cnt +1;
        end
        if(p_cnt == 1e4)
            disp('agc locked');
            locked_index = i;
        end

        g = g + K*(A-(z+2*g)); % gain loop
        
        p_sum(i) = sum(window);
        p_mean_pwr(i) = p_sum(i)/N;
        p_z(i) = z;
        p_g(i) = exp(g/2^11);
    end
end
figure;
subplot(411);plot(p_sum);
subplot(412);plot(p_mean_pwr);
subplot(413);plot(p_z);
subplot(414);plot(p_g);

figure;
subplot(221);plot(real(data));hold on;plot(locked_index,real(data(locked_index)),'ro');hold off;
subplot(223);plot(real(yout));hold on;plot(locked_index,real(yout(locked_index)),'ro');hold off;
subplot(222);plot(imag(data));hold on;plot(locked_index,imag(data(locked_index)),'ro');hold off;
subplot(224);plot(imag(yout));hold on;plot(locked_index,imag(yout(locked_index)),'ro');hold off;

figure;plot(p_temp);hold on;plot(locked_index,p_temp(locked_index),'ro');hold off;